home *** CD-ROM | disk | FTP | other *** search
/ Aminet 7 / Aminet 7 - August 1995.iso / Aminet / misc / sci / RARS_Amiga_3.lha / RARS / valerie.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-05-27  |  10.4 KB  |  223 lines
  1. // Valerie.CPP - "driver" function for RARS - M. Timin, April. 1995
  2. // adapted to ver. 0.39 3/6/95 by M. Timin
  3. // Copied from CNTRL0.CPP after version 0.50, bias added to steering servo
  4. // for ver. 0.60
  5.  
  6.              /* Easy-To-Understand Robot Driver */
  7. /*
  8. This robot driver attempts to stay in the middle of the track at all
  9. times.  She calculates a cornering speed for each corner based on its
  10. radius.  She accelerates on each straightaway until a certain fraction
  11. of its length is reached; Then she slows down, attempting to arrive at
  12. the corner with the proper cornering speed.  In the corner she attempts
  13. to maintain the cornering speed while attempting to stay in the middle
  14. of the track.  Her strategy for passing is to choose right or left at
  15. random, and then throw the car into a sharp slide toward that direction.
  16. */
  17. // The language here can be considered to be ANSI C or C++.
  18.  
  19. #include <string.h>
  20. #include <stdlib.h>
  21. #include <math.h>
  22. #include "car.h"
  23.  
  24. // These parameters may be adjusted to get better performance:
  25. const double CORN_SPD_CON = 5.9;   // determines how fast to take corners
  26. const double STEER_GAIN = 0.4;     // servo gain, for staying in "lane"
  27. const double STEER_DAMP = 1.2;     // servo damping, to prevent "weaving"
  28. const double END_ACCEL = .30; // we accelerate until this fraction of length
  29. const double END_CORNER = 2.2;// used to decide when to start leaving the corer
  30. const double ESC_ALPHA = .03; // after a crash, set alpha like this, + or -
  31. const double SLIP_LIM = 9.0;  // maximum wheel slip, ft/sec, in wheel_slip()
  32. const double SLIP_CON = 200.0; // in formula for tire slip when accelrating
  33. const double SHARP_TURN = .1; // change in alpha when attempting to pass
  34. const double BIAS_CON = 9.0;  // in bias formula.  (see bias)
  35. const double CUT_OVER = -2.0; // multipy bias times this to change direction
  36. const double FROM_RAIL= 18.0; // target distance from inside rail in corners
  37. const int PASSING_TIME = 50;  // time to stay in passing maneuver, counts
  38.  
  39. // The following function calculates the speed for a corner.
  40. // The lateral force produced by cornering is proportional to the square
  41. // of the speed, and is inversely proportional to the radius of the path.
  42. // Therefore, the attainable cornering speed for differnt radii is
  43. // proportional to the square root of the radius.  This function implements
  44. // that rule.  The value to use for CORN_SPD_CON can be determined by
  45. // trial and error.  Example result: if the car is following a path with
  46. // radius of 100 ft, and if CORN_SPD_CON is 5.0, then corner_spd is 50 ft/sec.
  47. double corner_spd(double radius)
  48. {
  49.    if(radius == 0.0)   // This is just insurance, this funtion doesn't
  50.       return(250.0);        // make sense when the radius is zero.
  51.    return CORN_SPD_CON * sqrt(radius);
  52. }
  53.  
  54. // In order to set vc, if you know how fast you want to go (goal), and how
  55. // fast you are going now (present), This function will compute a reasonable
  56. // value for vc.  The value is never very far from the present speed, both
  57. // to attempt to stay within the power limit, and to maintain steering control.
  58. // You can adjust the resulting slip by changing SLIP_LIM.
  59. double tire_speed(double goal, double present)
  60. {
  61.    double ws;
  62.  
  63.    if(present > goal + 2 * SLIP_LIM)  // if too fast,
  64.       ws = present - SLIP_LIM;      // slow down.
  65.    else if(present < goal - 2 * SLIP_LIM)  // if too slow,
  66.       ws = present + SLIP_LIM;             // accelerate.
  67.    else                           // if quite close,
  68.       ws = (goal + present) / 2;      // approach desired speed gently.
  69.  
  70.    return ws;
  71. }
  72.  
  73. /* These two structures from CAR.H are repeated here as comments, because
  74.    the "driver" function receives situation as input and produces con_vec
  75.    as output.
  76. struct situation {       // a car's local situation as seen by the driver
  77.    double cur_rad;       // radius of inner wall of curve (0 means straight)
  78.    double cur_len;       // length of current track segment (angle if curve)
  79.    double to_lft;        // distance to left wall
  80.    double to_rgt;        // distance to right wall
  81.    double to_end;        // how far to end of current track seg. (angle or feet)
  82.    double v;             // the speed of the car, feet per second
  83.    double vn;            // component of v perpendicular to track direction
  84.    double nex_len;       // length of the next track segment (angle if curve)
  85.    double nex_rad;       // radius of inner wall of next segment (or 0)
  86.    double after_rad;     // radius of the segment after that one. (or 0)
  87.    double power_req;     // ratio: power requested by driver to maximum power
  88.    int dead_ahead;       // set when there is a car dead ahead, else 0
  89. };
  90. struct con_vec { double alpha, vc; };  // control vector, steering & throttle
  91. */
  92.  
  93. // The task of this function is to compute vc and alpha.  A high speed
  94. // car on a track is a little like the keel of a boat; if you set the keel
  95. // at a slight angle to the direction of the oncoming water, you get a large
  96. // force to the side.  That is how we corner the car.  The driver sets the
  97. // car at a slight angle with respect to its direction of motion, this
  98. // cause a force to the side, causing the path of the car to curve.  The
  99. // magnitude of the force is proportional to the angle (alpha) for very
  100. // small alpha, and when there is not much wheel spin.  The wheel spin
  101. // is controlled by vc, which is the rearward speed of the bottom of the
  102. // tire.  When going down the straight at a constant, moderate velocity,
  103. // then vc is equal to the speed of the car.  For acceleration, vc is
  104. // made a little greater than the speed.  For braking, it is made a little
  105. // less.  When accelerating, vc is limited by the power available.
  106. con_vec Valerie(situation& s)
  107. {
  108.    const char name[] = "Valerie";     // This is the robot driver's name!
  109.    static int init_flag = 1;          // cleared by first call
  110.    double speed;                      // target speed for cornering, ft/sec
  111.    double speed_next;                 // target speed for next corner
  112.    con_vec result;                    // This is what is returned.
  113.    double bias;                // estimate of alpha required for corner
  114.    double lane;      // for steering servo, the target distance from left rail
  115.    double width;                      // track width, feet
  116.    double alpha, vc;           // components of result
  117.    static double lane_was = FROM_RAIL;  // previous turn's lane value (on straight)
  118.    static double alpha_inc = 0.0;  // alpha increment during passing maneuver
  119.    static int counting = 0;    // will be set and counting down when passing
  120.    int corner_ending;          // flag to signal nearing end of corner  
  121.  
  122.    // This paragraph has nothing to do with car control; it is just
  123.    // to identify the driver by copying its name to a global RAM area:
  124.    // This happens only on the very first call to this function
  125.    if(init_flag)  {            // first time through, only copy name:
  126.       my_name_is(name);        // copy the name string into the host program
  127.       init_flag = 0;
  128.       result.alpha = result.vc = 0;
  129.       return result;
  130.    }
  131.  
  132.   if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
  133.       return result;
  134.  
  135.    width = s.to_lft + s.to_rgt;                        // find width of track
  136.  
  137.    // decide whether or not to set corner_ending flag:
  138.    if(s.cur_rad != 0.0)
  139.       if(s.to_end * (fabs(s.cur_rad) + FROM_RAIL) < END_CORNER * width)
  140.          corner_ending = 1;     // means near end of corner
  141.       else
  142.          corner_ending = 0;     // not near end of corner
  143.    else
  144.       corner_ending = -1;       // this means we are not in a corner
  145.  
  146.    // choose a value for the lane variable:
  147.    if(s.cur_rad > 0.0)
  148.       lane = lane_was = FROM_RAIL;
  149.    else if(s.cur_rad < 0.0)
  150.       lane = lane_was = width - FROM_RAIL;
  151.    else
  152.       if(s.nex_rad > 0.0)
  153.          if(lane_was == FROM_RAIL)
  154.             lane = lane_was;
  155.          else
  156.             lane = (s.to_end/s.cur_len)*(width - 2 * FROM_RAIL) + FROM_RAIL;
  157.       else
  158.          if(lane_was == FROM_RAIL)
  159.             lane = width - FROM_RAIL - (s.to_end/s.cur_len)*(width - 2 * FROM_RAIL);
  160.          else
  161.             lane = lane_was;
  162.  
  163.    // Set alpha based on a servo-mechanism approach, trying to stay
  164.    // in the middle of the track, i.e., s.to_left equal to .5 * width:
  165.    alpha = STEER_GAIN * (s.to_lft - lane) / width;
  166.    alpha -= STEER_DAMP * s.vn / s.v; // This is damping,to prevent oscillation
  167.  
  168.    // calculate target speeds for current corner and the next:
  169.    if(s.nex_rad > 0.0)
  170.       speed_next = corner_spd(s.nex_rad + FROM_RAIL);
  171.    else if(s.nex_rad < 0.0)
  172.       speed_next = corner_spd(-s.nex_rad + FROM_RAIL);
  173.    else
  174.       speed_next = 250.0;
  175.  
  176.    if(s.cur_rad > 0.0)
  177.       speed = corner_spd(s.cur_rad + FROM_RAIL);
  178.    else if(s.cur_rad < 0.0)
  179.       speed = corner_spd(-s.cur_rad + FROM_RAIL);
  180.    else
  181.       speed = 250.0;
  182.  
  183.    // if in a turn, adjust the alpha calculation to a likely estimated value:
  184.    // (This is done because the servo would otherwise have to hunt for it.)
  185.    if(s.cur_rad != 0.0)   {
  186.       bias = BIAS_CON * (s.v/speed) / speed;  // an empirical formula
  187.       if(s.cur_rad < 0.0)
  188.          bias = -bias;
  189.       if(corner_ending && s.cur_rad * s.nex_rad < 0.0)
  190.          // cut over toward next corner when this happens
  191.          bias = CUT_OVER * bias;
  192.       alpha += bias;
  193.    }
  194.  
  195.    // now set the tire speed, vc:
  196.    if(s.cur_rad == 0.0)                  // If we are on a straightaway,
  197.       if(s.to_end > END_ACCEL * s.cur_len) // if we are far from the end,
  198.          vc = s.v + SLIP_CON / s.v;          // keep accellerating near full power
  199.       else                    // otherwise,
  200.          vc = tire_speed(speed_next, s.v);    // brake for next corner
  201.    else                              // If we're in the curve, maintain speed.
  202.       if(corner_ending) // When we near the next corner, adjust to "speed_next"
  203.          vc = tire_speed(speed_next, s.v);
  204.       else              // if we are far from the next corner, stay at "speed".
  205.          vc = tire_speed(speed, s.v);
  206.  
  207.    // The passing maneuver:
  208.    if(s.dead_ahead & !counting)  {  // When first encountering the car ahead:
  209.       counting = PASSING_TIME;         // setup the timer,
  210.       if(rand() < RAND_MAX/2)        // choose a right or left maneuver:
  211.          alpha_inc = SHARP_TURN;
  212.       else
  213.          alpha_inc = -SHARP_TURN;
  214.    }
  215.    if(counting)  {                // If we are still in the passing maneuver,
  216.       alpha += alpha_inc;              // change alpha
  217.       --counting;                      // count down to zero
  218.    }
  219.  
  220.    result.vc = vc;   result.alpha = alpha;
  221.    return result;
  222. }
  223.